package parallel.project.graph;

import java.awt.Point;
import java.awt.Rectangle;
import java.io.FileWriter;
import java.io.IOException;

import java.io.PrintWriter;
import java.io.Serializable;
import java.util.ArrayList;
import java.util.Collections;
import java.util.HashMap;
import java.util.HashSet;
import java.util.List;
import java.util.Set;
import java.util.TreeSet;

import parallel.project.framework.Serializer;

/**
 * Defines a graph composed of {@link Node}s connected by {@link Edge}s.
 * Every {@link Node} in the graph is identified by a unique name.  All
 * {@link Edge}s connect exactly 2 {@link Node}s and are associated with
 * a numerical weight.  All {@link Node}s and {@link Edge}s can also
 * have maze grid 2D coordinate locations.
 * 
 * @author Adam Clayton
 * @author Larry Walters
 */
public class Graph implements Serializable
{
	/** The {@link Node}s in the graph */
	private List<Node> nodes;
	
	/** The {@link Edge}s in the graph */
	private List<Edge> edges;
	
	/** All the starting location points */
	private List<Point> starts;
	
	/** All the ending location points */
	private List<Point> ends;
	
	/**
	 * Creates a new {@link Graph} with no {@link Node} and no
	 * {@link Edge}s
	 */
	public Graph() 
	{
		this.nodes = new ArrayList<Node>();
		this.edges = new ArrayList<Edge>();
		this.starts = new ArrayList<Point>();
		this.ends = new ArrayList<Point>();
	}
	
	/**
	 * Adds all the {@link Node}s, {@link Edge}s, starting points, and
	 * ending points from the given graph to this one, if they are not
	 * already in this one.  Note that no connections are made unless
	 * they are inherent in the graph being added (i.e. there is
	 * overlap.  If the graphs are disjoint, then there is no merging.
	 * 
	 * @param graph
	 * 		Graph to add to this one
	 */
	public void addGraph(Graph graph)
	{
		// go through all the nodes in the other graph and add them to
		// this graph if they are not already in this graph
		for (Node otherNode : graph.nodes)
		{
			if (!this.nodes.contains(otherNode))
				this.nodes.add(otherNode);
		}
		// go through all the edges in the other graph and add them to
		// this graph if they are not already in this graph
		for (Edge otherEdge : graph.edges)
		{
			if (!this.edges.contains(otherEdge))
				this.edges.add(otherEdge);
		}
		// go through all the start points in the other graph and add 
		// them to this graph if they are not already in this graph
		for (Point otherPoint : graph.starts)
		{
			if (!this.starts.contains(otherPoint))
				this.starts.add(otherPoint);
		}
		// go through all the end points in the other graph and add 
		// them to this graph if they are not already in this graph
		for (Point otherPoint : graph.ends)
		{
			if (!this.ends.contains(otherPoint))
				this.ends.add(otherPoint);
		}
	}
	
	/**
	 * Adds an {@link Edge} to the graph between the two given 
	 * {@link Node}s with the given weight, and which covers the 
	 * coordinate locations in the given list.  The list can be null,
	 * and then the edge will cover no locations.  Both {@link Node}s
	 * must already be in the graph, otherwise can exception will be
	 * thrown.
	 * 
	 * @param from
	 * 		Node to start the {@link Edge} at, for a directed graph; in
	 * 		an undirected graph it is just one of the two
	 * @param to
	 * 		Node to end the {@link Edge} at, for a directed graph; in
	 * 		an undirected graph it is just one of the two
	 * @param weight
	 * 		Numerical weight of the {@link Edge}
	 * @param locations
	 * 		Maze coordinate locations that are covered by this edge,
	 * 		ordered as going from the <code>from</code> {@link Node} to
	 * 		the <code>to</code> {@link Node}; so the location at
	 * 		<code>locations[0]</code> comes right after the 
	 * 		<code>from</code> {@link Node}, <code>locations[1]</code> 
	 * 		comes next, and so on up till 
	 * 		<code>locations[locs.size()-1]</code> which is right
	 * 		before the location of the <code>to</code> {@link Node}.
	 * 
	 * @return
	 * 		The {@link Edge} created and added
	 * @throws GraphException
	 */
	public Edge addEdge(Node from, Node to, Double weight, 
		List<Point> locations) throws GraphException
	{
		// check that both nodes are not null
		if (from == null && to == null)
		{
			throw new GraphException ("Can't add edge, both nodes are" +
				" null");
		}	
		else if (from == null)
		{
			throw new GraphException ("Can't add edge, from node is" +
				" null");
		}
		else if (to == null)
		{
			throw new GraphException ("Can't add edge, to node is" +
				" null");
		}
		else {}
		// check that both nodes are in the graph already, if not, throw
		// an exception
		Node tmpFrom = this.findNode(from.getName());
		Node tmpTo = this.findNode(to.getName());
		if (tmpFrom == null && tmpTo == null)
		{
			throw new GraphException("Both nodes " + from.getName() + 
				" and " + to.getName() + " are not in the graph, so " +
				"can't add edge");
		}
		else if (this.findNode(from.getName()) == null)
		{
			throw new GraphException("Node " + from.getName() + " is" +
				" not in the graph, so can't add edge");
		}
		else if (this.findNode(to.getName()) == null)
		{
			throw new GraphException("Node " + to.getName() + " is" +
				" not in the graph, so can't add edge");
		}
		else {}
		// if the locations list is null, make the list an empty one
		if (locations == null)
			locations = new ArrayList<Point>();
		
		// checks pass, so make the edge between the nodes and add to 
		// the edge list
		Edge newEdge = new Edge(from,to,weight,locations);
		this.edges.add(newEdge);
		// add this edge to each of the nodes involved
		from.addEdge(newEdge);
		to.addEdge(newEdge);
		return newEdge;
	}
	
	/**
	 * Creates a new {@link Node} and adds it to the graph, connecting
	 * it to the {@link Node}s in the given {@link List}.  If the name
	 * is not unique (i.e. another {@link Node} in this {@link Graph}
	 * already has that name) then the operation will fail and throw
	 * an exception.  Otherwise the appropriate {@link Edge}s will be
	 * created, and the {@link Node} connected.  {@link Node}s are
	 * connected such that if this is a directed graph, the new
	 * {@link Node} will have all the {@link Edge}s coming into it (i.e.
	 * they are starting from the existing {@link Node}s).
	 * <br><br>
	 * If the given name is null, then the string representation of the 
	 * location will be used for the name, but the name and location
	 * cannot both be null.
	 * 
	 * @param newNodeName
	 * 		Name of the new node, must be unique in the {@link Graph}
	 * @param location
	 * 		Coordinate location of the {@link Node} in the maze
	 * @param connectTo
	 * 		All the {@link Node}s that this new {@link Node} should be
	 * 		connected to.  If any of these nodes are not a part of the
	 * 		graph already, then an exception is thrown
	 * @param weights
	 * 		All the weights for the {@link Edge}s between the new 
	 * 		{@link Node} and the given current {@link Node}s; stored at
	 * 		the same index as the index in the <code>connectTo</code> 
	 * 		list for the {@link Node} that the {@link Edge} with that 
	 * 		weight will be connected with.  I.e. 
	 * 		<code>connectTo[i]</code> will be connected to the new 
	 * 		{@link Node} with an {@link Edge} whose weight is 
	 * 		<code>weights[i]</code>
	 * @param locations
	 * 		All the maze grid coordinate locations for every 
	 * 		{@link Edge} to be created, stored at each index in	order 
	 * 		from the current node to the new node; stored such that
	 * 		<code>connectTo[i]</code> will be connected to the new 
	 * 		{@link Node} with an {@link Edge} whose weight is 
	 * 		<code>weights[i]</code> and which covers all the locations
	 * 		at <code>locations[i]</code>, where
	 * 		<code>locations[i][0]</code> is the first coordinate spot 
	 * 		away from the node at <code>connectTo[i]</code>, 
	 * 		<code>locations[i][1]</code> is the second, and so on so
	 * 		that <code>locations[i][locations[i].size()-1]</code> is
	 * 		the last coordinate spot before the location of the new 
	 * 		node (<code>location</code>)
	 * 
	 * @return
	 * 		The {@link Node} created and added
	 * @throws GraphException
	 * 		If the lists are of different sizes or the name is not
	 * 		unique
	 */
	public Node addNode(String newNodeName, Point location, 
		List<Node> connectTo, List<Double> weights, 
		List<List<Point>> locations) throws GraphException
	{
		// check if the name is null, in which case use the location 
		// as the name
		if (newNodeName == null)
		{
			if (location == null)
			{
				throw new GraphException("Node name and location " +
					"both null");
			}
			else
				newNodeName = new Node(location).getName();
		}
		// check if a node with this name already exists
		if (this.findNode(newNodeName) != null)
		{
			throw new GraphException("Node with name " + newNodeName +
				" already exists");
		}
		// if both the nodes to connect to and the weights are null,
		// meaning this node is just added as a lone spot in the graph,
		// then make those lists empty to pass the size check
		if (connectTo == null && weights == null)
		{
			connectTo = new ArrayList<Node>();
			weights = new ArrayList<Double>();
		}
		// check if the lists are the same size
		if (connectTo.size() != weights.size())
		{
			throw new GraphException("Node list and weights list are" +
				" not of the same size: node list size = " + 
				connectTo.size() + " weights list size = " + 
				weights.size());
		}
		// if the locations list is not null, then make sure it is the
		// same size as the node and weights lists
		if (locations != null)
		{
			// we have confirmed they other lists are the same size, so 
			// doesn't matter which one used to compare
			if (connectTo.size() != locations.size())
			{
				throw new GraphException("Locations list is not the " +
					"same size as the node and weights lists: node " +
					"and weights list size = " + connectTo.size() + 
					" locations list size = " + locations.size());
			}
		}
		// check if all nodes in the connectTo list exist in this graph
		// if the node to connect to is not in the graph, then 
		// throw an exception
		for (Node node : connectTo)
		{
			if (this.findNode(node.getName()) == null)
			{
				throw new GraphException("Node \'" + node.getName() + 
					"\' to connect to does not exist in graph");
			}
		}
		
		// checks pass, so create the node and put it in the node list
		Node newNode = new Node(newNodeName,location);
		this.nodes.add(newNode);
		// go through the lists (which are the same size) and for
		// each node create the needed edge with the given weight
		for (int i = 0; i < connectTo.size(); i++)
		{
			Node node = connectTo.get(i);
			// make the edge between the nodes and add to the edge list
			if (locations == null)
				this.addEdge(node,newNode,weights.get(i),null);
			else
			{
				this.addEdge(node,newNode,weights.get(i),
						locations.get(i));
			}
		}
		return newNode;
	}
	
	/**
	 * Adds the given point to the graph as a starting location, so a
	 * {@link Node} with that location will be considered a starting
	 * spot.
	 * 
	 * @param start
	 * 		Starting position
	 */
	public void addStartPoint(Point start)
	{
		this.starts.add(start);
	}

	/**
	 * Adds the given point to the graph as an ending location, so a
	 * {@link Node} with that location will be considered an ending
	 * spot.
	 * 
	 * @param end
	 * 		Ending position
	 */
	public void addEndPoint(Point end)
	{
		this.ends.add(end);
	}
	
	/**
	 * Returns the {@link Node} with the given name or null if it is
	 * not in the graph.
	 * 
	 * @param name
	 * 		Name of the {@link Node} to search for
	 * @return
	 * 		The {@link Node} with that name, or null if it was not
	 * 		found
	 */
	public Node findNode(String name)
	{
		// look through all the nodes to get the one with that name
		for (Node node : this.nodes)
		{
			if (node.getName().equals(name))
				return node;
		}
		// if get to this point, then no Node with that name
		return null;
	}
	
	/**
	 * Extracts all the {@link Node}s with exactly the given number of
	 * {@link Edge}s.  In the case of a directed graph, this includes
	 * all {@link Edge}s going in and coming out.
	 * 
	 * @param edgeCount
	 * 		Number of {@link Edge}s
	 * @param excluded
	 * 		List of {@link Node}s that even if they have the given
	 * 		number of {@link Edge}s, should not be included in the
	 * 		returned list
	 * 
	 * @return
	 * 		All {@link Node}s with exactly that number of {@link Edge}s
	 */
	public List<Node> findNodesEdgeCount(int edgeCount, 
		List<Node> excluded)
	{
		// list to store found nodes in
		List<Node> foundNodes = new ArrayList<Node>();
		// go through the list of nodes and extract all with the given
		// number of edges
		for (Node node : this.nodes)
		{
			// don't include the node if it is in the excluded list
			if (node.getEdgeCount() == edgeCount && 
				!excluded.contains(node))
				foundNodes.add(node);
		}
		// return the list of found nodes
		return foundNodes;
	}
	
	/**
	 * Returns all {@link Node}s as read-only
	 * 
	 * @return
	 * 		All {@link Node}s in the graph
	 */
	public List<Node> getAllNodes()
	{
		return Collections.unmodifiableList(this.nodes);
	}
	
	/**
	 * Returns all {@link Edge}s as read-only
	 * 
	 * @return
	 * 		All {@link Edge}s in the graph
	 */
	public List<Edge> getAllEdges()
	{
		return Collections.unmodifiableList(this.edges);
	}
	
	/**
	 * Returns all the starting locations as read-only
	 * 
	 * @return
	 * 		All the starting points in the graph
	 */
	public List<Point> getAllStartPoints()
	{
		return Collections.unmodifiableList(this.starts);
	}
	
	/**
	 * Returns all the ending locations as read-only
	 * 
	 * @return
	 * 		All the ending points in the graph
	 */
	public List<Point> getAllEndPoints()
	{
		return Collections.unmodifiableList(this.ends);
	}
	
	/**
	 * Returns all the {@link Node}s at start locations
	 * 
	 * @return
	 * 		All the starting {@link Node}s
	 */
	public List<Node> getAllStartNodes()
	{
		List<Node> startingNodes = new ArrayList<Node>();
		for (Node node : this.nodes)
		{
			if (this.starts.contains(node.getLocation()))
				startingNodes.add(node);
		}
		return startingNodes;
	}
	
	/**
	 * Returns all the {@link Node}s at end locations
	 * 
	 * @return
	 * 		All the ending {@link Node}s
	 */
	public List<Node> getAllEndNodes()
	{
		List<Node> endingNodes = new ArrayList<Node>();
		for (Node node : this.nodes)
		{
			if (this.ends.contains(node.getLocation()))
				endingNodes.add(node);
		}
		return endingNodes;
	}
	
	/**
	 * Returns all the {@link Node}s whose locations fall in the column
	 * given (i.e. their x coordinate equals the given column).
	 * 
	 * @param column
	 * 		The column to get {@link Node}s at
	 * @return
	 * 		List of all the {@link Node}s on that column
	 */
	public List<Node> getNodesOnColumn(int column)
	{
		List<Node> columnNodes = new ArrayList<Node>();
		for (Node node : this.nodes)
		{
			if (node.getLocation().x == column)
				columnNodes.add(node);
		}
		return columnNodes;
	}
	
	/**
	 * Prunes the graph in the following way.  Disregarding 
	 * {@link Node}s whose location falls on the boundary of the given
	 * {@link Rectangle}, finds all {@link Node}s with only 1 
	 * {@link Edge} and removes them from the graph.  It repeats the 
	 * process until there are no more {@link Node}s to remove.  Then,
	 * disregarding the boundary {@link Node}s again, it then finds all
	 * the {@link Node}s with 2 {@link Edge}s and removes them,
	 * merging all the {@link Edge}s of each individual {@link Node} it
	 * removes.  The result is a graph with no dead ends and all 
	 * intermediate {@link Node}s replaced with merged {@link Edge}s.
	 * 
	 * @param bounds
	 * 		The maze boundary of the graph section, so that any
	 * 		{@link Node}s falling on this boundary are not pruned
	 * @throws GraphException
	 */
	public void prune(Rectangle bounds) throws GraphException
	{
		// all nodes on the edges of a maze should not be pruned cause
		// they may need to be hooked up to other mazes, so find all
		// the boundary nodes
		List<Node> boundaryNodes = new ArrayList<Node>();
//		System.out.println("Boundary " + bounds.toString());
		for (Node node : this.nodes)
		{
			Point loc = node.getLocation();
//			System.out.println("\tNode " + loc.toString());
			// add all nodes on the bounds (need -1 cause of array
			// locations starting at 0), only need the column boundary
			// nodes, cause the maze is split by columns
			if (loc.x == bounds.x || 
				loc.x == bounds.x + bounds.width - 1/* ||
				loc.y == bounds.y || 
				loc.y == bounds.y + bounds.height - 1*/)
				boundaryNodes.add(node);
			else if (this.starts.contains(node.getLocation()) ||
					 this.ends.contains(node.getLocation()))
			{
//				System.out.println("start or end " + node.toString());
				boundaryNodes.add(node);
			}
			else {/*System.out.println("Not added");*/}
		}
/*		if (this.starts.size() > 0)
			System.out.println("Start " + this.starts.get(0).toString());
		if (this.ends.size() > 0)
			System.out.println("End " + this.ends.get(0).toString());
		System.out.println(Thread.currentThread().getName() + 
			" boundary nodes:");
		System.out.println(boundaryNodes.toString());*/
		// while possible, remove all nodes with only 1 edge cause know
		// that they are a deadend (this does not include the boundary
		// nodes so they can have 1 edge)
		List<Node> oneEdge = this.findNodesEdgeCount(1,boundaryNodes);
		while (oneEdge.size() > 0)
		{
			for (Node oneEdgeNode : oneEdge)
				this.removeNode(oneEdgeNode);
			oneEdge = this.findNodesEdgeCount(1,boundaryNodes);
		}
		// while possible, remove all nodes with 2 edges, cause know
		// that they are just an intermediate point between two other
		// nodes, so just connect those two other nodes (this does not
		// include the boundary nodes, so they can have 2 edges)
		List<Node> twoEdges = this.findNodesEdgeCount(2,boundaryNodes);
		while (twoEdges.size() > 0)
		{
			for (Node twoEdgeNode : twoEdges)
				this.removeNodeConnecting(twoEdgeNode);
			twoEdges = this.findNodesEdgeCount(2,boundaryNodes);
		}
	}
	
	/**
	 * Removes all the starting locations (not any {@link Node}s, just
	 * the indication that certain locations are starting spots).
	 */
	public void removeAllStartPoints()
	{
		this.starts.clear();
	}

	/**
	 * Removes all the ending locations (not any {@link Node}s, just
	 * the indication that certain locations are ending spots).
	 */
	public void removeAllEndPoints()
	{
		this.ends.clear();
	}
	
	/**
	 * Removes the given {@link Node} from the graph, removing all the
	 * {@link Edge}s as well that are associated with that {@link Node}.
	 * 
	 * @param node
	 * 		{@link Node} to remove
	 * 
	 * @throws GraphException
	 */
	public void removeNode(Node node) throws GraphException
	{
		// check that the node is actually in the graph
		if (this.findNode(node.getName()) == null)
		{
			throw new GraphException("Can't remove node " + 
				node.getName() + " since it's not in the graph");
		}
		// take the node out of the list
		this.nodes.remove(node);
		// get all the nodes this node is/was connected to
		List<Node> otherNodes = new ArrayList<Node>();
		for (Edge edge : node.getEdges())
		{
			if (edge.getFirst() == node)
				otherNodes.add(edge.getSecond());
			else
				otherNodes.add(edge.getFirst());
		}
//		System.out.println("e:\n" + node);
//		System.out.println(otherNodes.toString());
		// remove all the edges associated with this node from all the
		// other nodes
		for (Node n : otherNodes)
			n.removeEdges(node.getEdges());
//		System.out.println(otherNodes.toString());
		// also remove the edges from the list here
		this.edges.removeAll(node.getEdges());
//		System.out.println(this.edges.toString());
	}
	
	/**
	 * Removes the given {@link Node} from the graph, and then 
	 * connects the {@link Edge}s associated with that {@link Node} to
	 * each other so that all the {@link Node}s that were connected to
	 * this {@link Node} are now connected to each other via merging 
	 * the {@link Edge}s together.
	 * 
	 * @param node
	 * 		{@link Node} to remove from the graph
	 * @throws GraphException
	 */
	public void removeNodeConnecting(Node node) throws GraphException
	{
		// remove the node as would normally without connecting the
		// edges
		this.removeNode(node);
		// now merge every edge of the removed node and add them to a
		// list which will be added to the graph as the new edges
		List<Edge> mergedEdges = new ArrayList<Edge>();
		List<Pair> donePairs = new ArrayList<Pair>();
		for (Edge edge1 : node.getEdges())
		{
			for (Edge edge2 : node.getEdges())
			{
				Pair tmp = new Pair(edge1,edge2);
				// if they are not the same edge, and haven't done this
				// combination before, then combine them and add the 
				// combined one to the list
				if (!edge1.equals(edge2) && !donePairs.contains(tmp))
				{
					mergedEdges.add(edge1.merge(edge2));
					donePairs.add(tmp);
				}
			}
		}
		// add all these new edges to the graph, this involves creating
		// new edge objects that are exactly the same, but oh well
		for (Edge newEdge : mergedEdges)
		{
			this.addEdge(newEdge.getFirst(),newEdge.getSecond(),
					newEdge.getWeight(),newEdge.getLocations());
		}
	}
	
	/**
	 * Prints the graph in .dot format for Graphviz
	 * 
	 * @param filename
	 * 		Name of the .dot format file (should include the extension)
	 * 
	 * @throws IOException
	 */
	public void toDot(String filename) throws IOException
	{
		this.toDot(filename,null);
	}
	
	/**
	 * Prints the graph in .dot format for Graphviz
	 * 
	 * @param filename
	 * 		Name of the .dot format file (should include the extension)
	 * @param path
	 * 		Path to highlight in the graph by making the edges bold;
	 * 		can be null, which will just display the graph normally
	 * 
	 * @throws IOException
	 */
	public void toDot(String filename, Path path) throws IOException
	{
		FileWriter fw = new FileWriter(filename);
		PrintWriter writer = new PrintWriter(fw);
		writer.println("graph {");
		// indicate the start and stop nodes are to be specially colored
		for (Node node : this.nodes)
		{
			if (this.starts.contains(node.getLocation()))
			{
				writer.println("\t" + node.getName() + 
					" [color=\"green\"]");
			}
			if (this.ends.contains(node.getLocation()))
			{
				writer.println("\t" + node.getName() + 
					" [color=\"red\"]");
			}
		}
		// list of nodes printed from being in an edge
		Set<Node> fromEdges = new HashSet<Node>();
		// go through all the edges and print them out to dot format
		for (Edge edge : this.edges)
		{
			String line = "\t" + edge.getFirst().getName() + " -- " + 
				edge.getSecond().getName();
			if (edge.getWeight() != 0.0)
				line += " [label=\"" + edge.getWeight() + "\"";
			if (path != null && path.containsEdge(edge))
				line += " style=\"bold\"]";
			else
				line += "]";
			line += ";";
			writer.println(line);
			// indicate these two nodes have been added to the file
			fromEdges.add(edge.getFirst());
			fromEdges.add(edge.getSecond());
		}
		// if the number of nodes added cause they were attached to
		// edges is not the same as the number of total nodes, then 
		// some were missed cause they were not attached to anything,
		// so add those to the file
		if (fromEdges.size() != this.nodes.size())
		{
			for (Node node : this.nodes)
			{
				if (!fromEdges.contains(node))
					writer.println("\t" + node.getName() + ";");
			}
		}
		// if there were no edges, just display the nodes
		if (this.edges.size() == 0)
		{
			for (Node node : this.nodes)
			{
				String line = "\t" + node.getName() + ";";
				writer.println(line);
			}
		}
		writer.println("}");
		writer.close();
	}
	
	/**
	 * Reads from the given file and returns the {@link Graph} object
	 * read.  The file should only contain a single {@link Graph} object
	 * written using {@link #writeGraph(String)}.  This uses basic Java
	 * serialization.
	 * 
	 * @param filename
	 * 		The name of the file to read the {@link Graph} from
	 * 
	 * @return
	 * 		The {@link Graph} object read from the file
	 * @throws ClassNotFoundException
	 * @throws IOException
	 */
	public static Graph readGraph(String filename) 
		throws ClassNotFoundException,IOException
	{
		return Serializer.readObject(new Graph(),filename);
	}
	
	/**
	 * Writes this {@link Graph} to the file with the given filename.  
	 * This should be the only thing written to this file, if you 
	 * intend to retrieve the contents at some other time with a call 
	 * to {@link #readGraph(String)}.  This uses basic Java 
	 * serialization.
	 * 
	 * @param filename
	 * 		The name of the file to write to
	 * @throws IOException
	 */
	public void writeGraph(String filename) throws IOException
	{
		Serializer.writeObject(this,filename);
	}
	
	// Taken from A-star pseudocode in
	// http://en.wikipedia.org/wiki/A*_search_algorithm
	public Path solveAStar(Node start, Node goal){
		HashSet<Node> closedset = new HashSet<Node>();
		HashSet<Node> openset = new HashSet<Node>(); 
		HashMap<Node, Edge> camefrom = new HashMap<Node, Edge>();
		
		openset.add(start);
		
		HashMap<Node, Double> g_score = new HashMap<Node, Double>();
		HashMap<Node, Double> h_score = new HashMap<Node, Double>();
		HashMap<Node, Double> f_score = new HashMap<Node, Double>();
		
		g_score.put(start, 0.0);
		h_score.put(start, start.getLocation().distance(goal.getLocation()));
		f_score.put(start, g_score.get(start)+h_score.get(start));
//		f_score.put(start, g_score.get(start)+f_score.get(goal));
		
		Node lowest_fscore = start;
		boolean tentative_is_better;
		while(!openset.isEmpty()){
			lowest_fscore = null;
			for(Node n : openset){
				if(lowest_fscore == null || f_score.get(lowest_fscore)>f_score.get(n))
					lowest_fscore = n;
			}
			if(lowest_fscore == goal)
				return reconstructPath(camefrom, start, goal);
//				return reconstructPath(camefrom, goal);
			
			openset.remove(lowest_fscore);
			closedset.add(lowest_fscore);
			
			for(Edge e : lowest_fscore.getEdges()){
				Node neighbor = e.getOtherNode(lowest_fscore);
				
				if(closedset.contains(neighbor))
					continue;
				
				double tentative_g_score = g_score.get(lowest_fscore) + lowest_fscore.getLocation().distance(neighbor.getLocation());
				
//				if(openset.contains(neighbor)){
				if(!openset.contains(neighbor)){
					openset.add(neighbor);
					tentative_is_better = true;
				} else if(tentative_g_score < g_score.get(neighbor)){
					tentative_is_better = true;
				} else {
					tentative_is_better = false;
				}
				
				if(tentative_is_better){
					camefrom.put(neighbor, e);
					g_score.put(neighbor, tentative_g_score);
//					h_score.put(neighbor, lowest_fscore.getLocation().distance(neighbor.getLocation()));
					h_score.put(neighbor, neighbor.getLocation().distance(goal.getLocation()));
					f_score.put(neighbor, g_score.get(neighbor)+h_score.get(neighbor));
				}
			}
			
		}
		return null;
	}
	
	private Path reconstructPath(HashMap<Node, Edge> camefrom, 
		Node start, Node goal) {
		ArrayList<Edge> edges = new ArrayList<Edge>();
		
		Node current = goal;//, other;
		Edge currentedge;
		do{
			currentedge = camefrom.get(current);
			if (currentedge == null)
				break;
			edges.add(currentedge);
			current = currentedge.getOtherNode(current);
		} while(current != goal);
		// TODO think this should be current != start
			
		Collections.reverse(edges);
		
		return new Path(edges,start,goal);
	}

	
	/**
	 * Class used to store 2 edges, for use when merging edges, and want
	 * to compare to see if they were already merged
	 */
	public class Pair
	{
		/** The edges of the pair */
		Set<Edge> edges;
		
		/**
		 * Create a new pair
		 * 
		 * @param one
		 * 		First edge
		 * @param two
		 * 		Second edge
		 */
		public Pair(Edge one, Edge two)
		{
			this.edges = new HashSet<Edge>();
			this.edges.add(one);
			this.edges.add(two);
		}
		
		@Override
		public boolean equals(Object o)
		{
			if (!(o instanceof Pair))
				return false;
			Pair other = (Pair)o;
			if (this.edges.size() == other.edges.size() &&
				this.edges.containsAll(other.edges))
				return true;
			else
				return false;
		}
	}
	
}
